#ifndef ISMFORTEST__ISMTEST_HPP_
#define ISMFORTEST__ISMTEST_HPP_

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp> 
#include "ismfortest/json_data.hpp"
#include <memory>
#include <chrono>
#include <string>

using namespace std::chrono_literals;

namespace ismtest{

    class ismtester : public rclcpp::Node
    {

    public:
        ismtester();

    private:
        // 定义状态枚举
        // enum class State
        // {
        //     // 空闲和运行状态
        //     STATE_IDLE,
        //     STATE_RECEIVING_COMMAND, 

        //     // 移动到库内
        //     STATE_ENTERING_WEARHOUSE,

        //     // 导航到指定井
        //     STATE_NAVIGATING_TO_WELL,
        //     STATE_ARRIVED_AT_WELL,

        //     // 操作井盖
        //     STATE_REMOVING_WELL,
        //     STATE_WELL_REMOVED,

        //     // 提取放射源
        //     STATE_EXTRACTING_SOURCE,
        //     STATE_SOURCE_EXTRACTED,

        //     // 送出放射源
        //     STATE_DELIVERING_SOURCE,
        //     STATE_SOURCE_DELIVERED,

        //     // 合上井盖
        //     STATE_REPLACING_WELL,
        //     STATE_WELL_REPLACED
        // };

        // 定义抓手状态枚举
        // enum class GripperState 
        // {
        //     NONE,
        //     SOURCE,
        //     COVER
        // };

        // ROS2相关
        rclcpp::TimerBase::SharedPtr timer_;
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_subscriber_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr system_status_publisher_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pose_publisher_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr task_status_publisher_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr gripper_status_publisher_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr well_status_publisher_;

        // 当前为提取/送回放射源
        bool is_return_operation_;

        // 当前state，传递状态值
        JsonData_convert::State current_state_;
        int state_timer_; // 后期可删除，为控制话题发布时间

        // ui控制变量
        std::string command_action_;
        int target_well_id_;
        
        // 系统状态变量
        std::string robot_id_;
        bool battery_charging_;
        double run_time_;

        // 位姿状态变量
        struct RobotPose 
        {
            double x;
            double y;
            double theta;
        };
        RobotPose robot_pose_;
        std::string frame_id_;

        // 任务状态变量
        std::string task_stage_;

        // 抓手状态实例
        JsonData_convert::GripperState gripper_loaded_;

        // 放射源坑状态变量
        int current_well_id_;
        bool current_well_has_source_;
        bool current_well_has_cover_;

        // JsonData_convert 实例
        JsonData_convert json_converter_;


        void commandCallback(const std_msgs::msg::String::SharedPtr msg);
        std::string getCurrentTimestamp();
        void publishSystemStatus();
        void publishPoseStatus();
        void publishTaskStatus();
        void publishGripperStatus();
        void publishWellStatus();
        void timerCallback();
        void processStateMachine();
    };
}


#endif